// Contact resolution system. These classes handle collisions, contacts,
// and other contraints. The resolver loops through each contact and 
// resolves it by manipulating the objects involved and using the data
// stored (such as the contact normal, friction, restitution, etc.)
#pragma once

#include "rigidbody.h"


#define VELOCITY_EPSILON		0.01
#define POSITION_EPSILON		0.01

namespace physics {
	/////////////////////////////////////////
	// Contact
	/////////////////////////////////////////
	// A Contact object stores the relevant data 
	// for two objects in contact with one another, which
	// can then be resolved by a contact resolution
	// algorithm.

	// defined further below
	// need this so we can make it a friend class
	// to the Contact class
	class ContactResolver;

	class Contact
	{
		
		

		// lets the ContactResolver play
		// with the Contact nicely
		friend ContactResolver;

	public:
		// holds the Particles involved in the
		// contact. The second particle could be
		// NULL in the case where we not colliding
		// two particles (e.g. collision between a
		// box and the floor)
		RigidBody *objects[2];

		// the coefficient of friction
		real friction;

		// the coefficient of restitution. Should be
		// based on the materials of the objects
		// involved and determined when this data is
		// created.
		real restitution;

		// the point of contact in world-coordinates
		Vector3 contactPoint;

		// the contact normal in world-coordinates.
		Vector3 contactNormal;
		
		// the penetration depth
		real penetration;
		
		// keeps track of how much the objects have
		// moved during contact resolution
		//Vector3 objectMovement[2];

		// Sets the data, such as the objects involved in the contact
		// and the frictional and resititional coefficients
		void setBodyData(RigidBody* one, RigidBody *two, real friction, real restitution);

	protected:
		// transform matrix that converts contact coordinates into
		// world coordinates
		Matrix3 contactToWorld;

		// the closing velocity at the point of contact
		Vector3 contactVelocity;

		// the required change in velocity for the contact to be resolved
		real desiredDeltaVelocity;

		// the world space positions of the contact point relative to
		// each body's COM.
		Vector3 relativeContactPosition[2];

		// Calculates important internal data.
		void calculateInternals(real dt);

		// reverses the contact. Swaps the two bodies and flips
		// the contact normal
		void swapBodies();

		// updates the awake state of the rigid bodies that are involved
		// in the contact. Will be made awake if it is in contact with
		// a body that is also awake.
		void matchAwakeState();

		//calculates the desired velocity to solve the contact
		void calculateDesiredDeltaVelocity(real dt);

		// calculates the velocity of hte contact point on the body
		Vector3 calculateLocalVelocity(unsigned bodyIndex, real dt);

		// calculates the orthonormal basis of the contact point
		void calculateContactBasis();

		// applies an impulse to the given body and returns
		// the change in velocity
		void applyImpulse(const Vector3 &impulse, RigidBody *body, Vector3 *velocityChange, Vector3 *rotationChange);

		// performs an impulse that is weighted by the object's interia
		void applyVelocityChange(Vector3 veocityChange[2], Vector3 rotationChange[2]);

		// performs a penetration resolution, weighted by teh object's intertia
		void applyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], real penetration);

		// calculates the impulse needed for contact resolution,
		// assuming zero friction.
		Vector3 calculateFrictionlessImpulse(Matrix3 *inverseInertiaTensor);

		// calculate the impulse needed for contact resolution
		// assuming a non-zero friction
		Vector3 calculateFrictionImpulse(Matrix3 *inverseInertiaTensor);

	};

	/////////////////////////////////////////
	// ContactResolver
	/////////////////////////////////////////
	// This object resolves Contact objects
	class ContactResolver
	{
	protected:
		// the number of iterations to perform when resolving velocity
		unsigned velocityIterations;

		// the number of iterations to perform when resolving postion
		unsigned positionIterations;

		// to avoid instability. Values smaller than this are considered
		// to be zero
		real velocityEpsilon;

		// as above, but for position
		real positionEpsilon;

	public:
		
		// resolves a set of contacts for penetration and velocity
		void resolveContacts(Contact *contactArray, 
							unsigned numContacts, 
							real dt);

		// the number of velocity iterations used
		unsigned velocityIterationsUsed;

		// the number of position iterations used
		unsigned positionIterationsUsed;

	private:
		// are the internal values valid?
		bool validSettings;

	public:
		// create a new contact resolver with optional epsilon values
		// and a finite number of iterations allowed
		ContactResolver(unsigned iterations, 
						real velocityEpsilon = (real)VELOCITY_EPSILON, 
						real positionEpsilon = (real)POSITION_EPSILON);

		// as above, but with specific values for velocity and positional iterations
		ContactResolver(unsigned velocityIterations,
						unsigned positionIterations,
						real velocityEpsilon = (real)VELOCITY_EPSILON, 
						real positionEpsilon = (real)POSITION_EPSILON);

		bool isValid()
		{
			return (velocityIterations > 0) &&
                   (positionIterations > 0) &&
                   (positionEpsilon >= 0.0f) &&
                   (positionEpsilon >= 0.0f);
        } 

		// set the number of iterations for each stage
		void setIterations(unsigned velocityIterations,
                           unsigned positionIterations);

		// set the number of iterations for both stages
		void setIterations(unsigned iterations);

		// set the tolerance values for velocity and position
		void setEpsilon(real velocityEpsilon,
                        real positionEpsilon);

	protected:
		// sets up teh contacts to be ready for processing. Makes sure
		// the internal data is configured correctly and objects are awake
		void prepareContacts(Contact *contactArray,
								unsigned numContacts,
								real dt);

		// resolves velocity issues
		void adjustVelocities(Contact *contactArray,
								unsigned numContacts,
								real dt);

		// resolves positional issues
		void adjustPositions(Contact* contactArray,
								unsigned numContacts,
								real dt);
	};

	class ContactGenerator
	{
	public:
		// Stores the given contact structure which can
		// later be iterated
		// through and have all of the contacts resolved.
		// has a finite limit on the number of contacts
		// that can be resolved in a single timestep and 
		// returns the number of contacts that have so
		// far been written this timestep. Is a polymorphic
		// interface for more complex contact generation systems
		virtual unsigned addContact(Contact *contact, unsigned limit) const = 0;
	};
}